#include "costmap_2d_ros.h"
#include <cstdio>
#include <string>
#include <algorithm>
#include <vector>
#include <tf2/convert.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/message_filter.h>
#include <sensor_msgs/point_cloud2_iterator.h>

namespace costmap_2d{

Costmap2DROS::Costmap2DROS(const std::string &name, tf2_ros::Buffer& tf):
    layered_costmap_(NULL),
    name_(name),
    tf_(tf),
    transform_tolerance_(0.3),
    publisher_(NULL){

    // 参数初始化
    ros::NodeHandle private_nh("~/" + name);
    private_nh.param("global_frame", global_frame_, std::string("map"));
    private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));
    private_nh.param("map_topic", map_topic_, std::string("/map"));
    private_nh.param("update_frequency", update_frequency_, 10.0);
    private_nh.param("transform_tolerance", transform_tolerance_, 0.2);

    // 初始化滚动窗口
    bool rolling_window_enable;
    double window_width, window_height, window_resolution;
    private_nh.param("rolling_window/enable", rolling_window_enable, false);
    private_nh.param("rolling_window/width", window_width, 4.0);
    private_nh.param("rolling_window/height", window_height, 4.0);
    private_nh.param("rolling_window/resolution", window_resolution, 4.0);
    LOG(INFO) << "rolling_window: " << (rolling_window_enable ? "true" : "false") 
        << ", width: " << window_width << ", height: " << window_height
        << ", resolution: " << window_resolution;
    layered_costmap_ = new LayeredCostmap(global_frame_, robot_base_frame_, rolling_window_enable);   
    publisher_ = new Costmap2DPublisher(layered_costmap_->getCostmap2D(), global_frame_, name_+"/costmap");
    map_sub_ = nh_.subscribe(map_topic_, 1, &Costmap2DROS::mapCallback, this);

    // 初始化图层
    bool static_layer_enable,sensors_layer_enable,inflation_layer_enable;
    private_nh.param("static_layer/enable", static_layer_enable, true);
    private_nh.param("sensors_layer/enable", sensors_layer_enable, true);
    private_nh.param("inflation_layer/enable", inflation_layer_enable, true);
    if(static_layer_enable){
        static_layer_ = boost::make_shared<StaticLayer>();      // 添加静态图层
        layered_costmap_->addPlugin(static_layer_);
        static_layer_->initialize(layered_costmap_, "static_layer", &tf_);
        LOG(INFO) << "static_layer initialized.";
    }
    if(sensors_layer_enable){
        sensors_layer_ = boost::make_shared<SensorsLayer>();    // 添加传感器图层
        layered_costmap_->addPlugin(sensors_layer_);
        sensors_layer_->initialize(layered_costmap_, "sensors_layer", &tf_); 
        LOG(INFO) << "sensors_layer initialized.";
    }
    if(inflation_layer_enable){
        inflation_layer_ = boost::make_shared<InflationLayer>();  // 添加膨胀图层
        layered_costmap_->addPlugin(inflation_layer_);
        inflation_layer_->initialize(layered_costmap_, "inflation_layer", &tf_);  
        LOG(INFO) << "inflation_layer initialized.";     
    }

    if(sensors_layer_enable){
        std::vector<std::string> sensor_list = parseSensorsList(private_nh);
        LOG(INFO) << "Found " << sensor_list.size() << " sensors:";
        for (const auto& name : sensor_list) {
            LOG(INFO) << "- " << name;
            std::string topic_name, data_type;
            double obstacle_range, raytrace_range, observation_keep_time;
            private_nh.param("sensors_layer/"+name+"/topic", topic_name, string(""));
            private_nh.param("sensors_layer/"+name+"/type", data_type, string(""));
            private_nh.param("sensors_layer/"+name+"/obstacle_range", obstacle_range, 2.5);
            private_nh.param("sensors_layer/"+name+"/raytrace_range", raytrace_range, 3.0);
            private_nh.param("sensors_layer/"+name+"/observation_keep_time", observation_keep_time, 0.0);
            LOG(INFO) << "sensor: " << name << ", topic: " << topic_name 
                << ", obstacle_range: " << obstacle_range 
                << ", raytrace_range: " << raytrace_range 
                << ", observation_keep_time: " << observation_keep_time;
        
            if(topic_name.empty() || data_type.empty()) continue;
    
            // 处理LaserScan数据
            if(data_type == "sensor_msgs/LaserScan"){
                boost::shared_ptr <message_filters::Subscriber<sensor_msgs::LaserScan>> sub(
                    new message_filters::Subscriber<sensor_msgs::LaserScan>(nh_, topic_name, 50));
                boost::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::LaserScan> > filter(
                    new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*sub, tf_, global_frame_, 50, nh_));
                filter->registerCallback([this, topic_name](auto& msg){ this->laserScanCallback(msg, topic_name); });
                filter->setTolerance(ros::Duration(0.05));
                // 添加到全局变量，避免循环结束，智能指针被释放
                observation_subscribers_.push_back(sub);
                observation_notifiers_.push_back(filter);
            }
    
            boost::shared_ptr < ObservationBuffer> buffer(
                new ObservationBuffer(topic_name, observation_keep_time, 
                    obstacle_range, raytrace_range, tf_, global_frame_, transform_tolerance_));
            sensors_layer_->createBuffer(buffer, topic_name);
            LOG(INFO) << "[createBuffer] topic_name: " << topic_name << endl;
        }
    }

    // 初始化滚动窗口
    if(rolling_window_enable){
        layered_costmap_->resizeMap((unsigned int)(window_width / window_resolution),
            (unsigned int)(window_height / window_resolution), window_resolution, 0.0, 0.0);
    }    
    map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROS::mapUpdateLoop, this, update_frequency_));
}

Costmap2DROS::~Costmap2DROS(){
    if(map_update_thread_ != NULL){
      map_update_thread_->join();
      delete map_update_thread_;
    }

    if(publisher_ != NULL)
        delete publisher_;

    if(layered_costmap_ != NULL)
        delete layered_costmap_;
}

LayeredCostmap* Costmap2DROS::getLayeredCostmap() const{
    return layered_costmap_;
}

std::vector<std::string> Costmap2DROS::parseSensorsList(ros::NodeHandle& nh){
    std::vector<std::string> sensors;

    // 从参数服务器获取 sensors_list 参数
    XmlRpc::XmlRpcValue sensors_list;
    nh.getParam("sensors_layer/sensors_list", sensors_list);

    // 检查参数类型是否为列表
    if (sensors_list.getType() != XmlRpc::XmlRpcValue::TypeArray) {
        ROS_ERROR("sensors_list is not a list type!");
        return sensors;
    }

    // 遍历列表元素
    for (int i = 0; i < sensors_list.size(); ++i) {
        if (sensors_list[i].getType() == XmlRpc::XmlRpcValue::TypeString) {
        sensors.push_back(static_cast<std::string>(sensors_list[i]));
        } else {
        ROS_WARN("Element %d in sensors_list is not a string", i);
        }
    }
    return sensors;
}

void Costmap2DROS::mapUpdateLoop(double frequency){
    ros::NodeHandle nh;
    ros::Rate r(frequency);
    while(ros::ok()){
        ros::spinOnce();
        geometry_msgs::PoseStamped pose;
        if(getRobotPose(pose)){
            double x = pose.pose.position.x,
                y = pose.pose.position.y,
                yaw = tf2::getYaw(pose.pose.orientation);
                layered_costmap_->updateMap(x, y, yaw);
            std::cout << "x: " << x << ", y: " << y << ", yaw: " << yaw << std::endl;
        }
        r.sleep();
        publisher_->publishCostmap();
    }
}

bool Costmap2DROS::getRobotPose(geometry_msgs::PoseStamped& global_pose) const{
    tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
    geometry_msgs::PoseStamped robot_pose;
    tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);
    robot_pose.header.frame_id = robot_base_frame_;
    robot_pose.header.stamp = ros::Time();
    ros::Time current_time = ros::Time::now();  // save time for checking tf delay later

    // get the global pose of the robot
    try
    {
        // use current time if possible (makes sure it's not in the future)
        if (tf_.canTransform(global_frame_, robot_base_frame_, current_time))
        {
        geometry_msgs::TransformStamped transform = tf_.lookupTransform(global_frame_, robot_base_frame_, current_time);
        tf2::doTransform(robot_pose, global_pose, transform);
        }
        // use the latest otherwise
        else
        {
        tf_.transform(robot_pose, global_pose, global_frame_);
        }
    }
    catch (tf2::LookupException& ex)
    {
        ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
        return false;
    }
    catch (tf2::ConnectivityException& ex)
    {
        ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
        return false;
    }
    catch (tf2::ExtrapolationException& ex)
    {
        ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
        return false;
    }
    // check global_pose timeout
    if (!global_pose.header.stamp.isZero() && current_time.toSec() - global_pose.header.stamp.toSec() > transform_tolerance_)
    {
        ROS_WARN_THROTTLE(1.0,
                        "Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f",
                        current_time.toSec(), global_pose.header.stamp.toSec(), transform_tolerance_);
        return false;
    }

  return true;
}

void Costmap2DROS::mapCallback(const nav_msgs::OccupancyGridConstPtr& new_map){
    static_layer_->setMap(new_map);
}

void Costmap2DROS::laserScanCallback(const sensor_msgs::LaserScanConstPtr& message, string topic_name){
    // LOG(INFO) << "[laserScanCallback]: " << topic_name;
    sensor_msgs::PointCloud2 cloud;
    cloud.header = message->header;
    try{
        projector_.transformLaserScanToPointCloud(message->header.frame_id, *message, cloud, tf_);
    }
    catch (tf2::TransformException &ex){
        ROS_WARN("High fidelity enable, but TF returned a transform exception to frame %s: %s", global_frame_.c_str(),
                ex.what());
        projector_.projectLaser(*message, cloud);
    }
    catch (std::runtime_error &ex){
        ROS_WARN("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s", ex.what());
        return; //ignore this message
    }  
    sensors_layer_->addPointCloud2(cloud, topic_name);
}

void Costmap2DROS::pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& message, string topic_name){

}

}   // namespace costmap_2d